perm filename SERVO.OLD[CMS,LCS]5 blob
sn#437667 filedate 1979-04-26 generic text, type T, neo UTF8
00100 TITLE SERVO
00200 .INSERT ASMBL.FAI[CMS,LCS]
00300
00400 ;Put HSTTMR in shared ram for host set?
00500
00600 ;I/O address definitions.
00700 DAC ← 100000 ;8 bit DAC.
00800 JCR ← 120000 ;Joint control output register.
00900 ENCL ← 140000 ;Encoder mux low.
01000 ENCH ← 140001 ;Encoder mux high.
01100
01200 STKSIZ ← 377 ;Stack size.
01300 LSBENB ← 40 ;Enable LSB servo.
01400
01500 ;Zero page variables.
01600 ;Not shared.
01700
01800 DSPAT: BLOCK 2 ;Dispatch address for commands.
01900 DEFCMD: 0 ;Deferred command.
02000 0 ;For 16 bit even addresses.
02100 SAVPOS: BLOCK 2 ;Position for deferred servo command.
02200
02300 CMDVEL: BLOCK 2 ;Commanded velocity.
02400 CURVEL: BLOCK 2 ;Current velocity.
02500 0 ;SETPT-1.
02600 SETPT: BLOCK 2 ;Current setpoint.
02700 0 ;SETINC-1.
02800 SETINC: BLOCK 2 ;Interpolating increment for setpoints.
02900 0 ;OLDINC-1.
03000 OLDINC: BLOCK 2 ;Last increment, for IVEL.
03100 OLDSP: BLOCK 2 ;Last commanded setpoint, for CMDVEL.
03200 POSERR: BLOCK 2 ;Current position error.
03300 DACSIG: BLOCK 2 ;Scratch.
03400
03500 INCTR: 0 ;Count the interpolations.
03600 HSTTMR: 0 ;Count ticks between host commands.
03700
03800 LOGTMP: BLOCK 4 ;Temp for the arithmetic routines.
03900
04000 IVEL: BLOCK 2 ;Interpolated velocity.
04100 VELERR: BLOCK 2 ;Velocity error term.
04200 VSUM: BLOCK 2 ;Sum of last 8 velocitys.
04300 VPTR: 0 ;Velocity averaging index.
04400 0
04500 VELTBL: BLOCK 20;Velocity averaging table.
04600
04700 ZAPEND ← .-1 ;Clear all the above in startup.
04800
04900 TL: 0 ;Scratch for gray to binary.
05000 TH: 0
05100
05200 FTMP: BLOCK 2 ;Copy of friction coefficient for multiply.
00100 ;Shared ram.
00200 LOC 200 ;Second half of zero page.
00300
00400 ;STATUS byte bits.
00500 ; 7 6 5 4 3 2 1 0
00600 ; check time no
00700 ; word out tick
00800
00900 0 ;Locked.
01000 STATUS: 0 ;Flags for the host.
01100
01200 ;MODE byte bits.
01300 ;Bit 7 6 5 4 3 2 1 0
01400 ; servo integ lsb
01500 ; enlb enlb enbl
01600
01700 0 ;Locked.
01800 MODE: 0 ;Mode bits from host.
01900
02000 CKWORD: BLOCK 2 ;Host I/O check/command word.
02100 CMDPOS: BLOCK 2 ;Commanded position from host.
02200
02300 ;IOCTRL byte bits.
02400 ;Bit 7 6 5 4 3 2 1 0
02500 ; in lsb integ pos
02600 ; tol enlb disabl mode
02700
02800 0 ;Locked.
02900 IOCTRL: 0 ;Copy of JCR output port.
03000
03100 CURPOS: BLOCK 2 ;Current position.
03200
03300 ;NINTER = function of INTSCL?
03400 0 ;Locked.
03500 NINTER: 0 ;# of interpolations between position
03600 ;commands.
03700 0 ;Locked.
03800 INTSCL: 0 ;# of bits to shift setpoint dif for
03900 ;interpolating.
04000 0 ;Locked.
04100 HSTLIM: 0 ;# of clock ticks allowed between host
04200 ;commands.
04300 FRICTN: BLOCK 2 ;Viscous damping coefficient.
04400 GRAVTY: BLOCK 2 ;DC offset for gravity.
04500 POSTOL: BLOCK 2 ;Half-width of position tolerance band.
04600 INTTOL: BLOCK 2 ;Half-width of integration band.
04700
04800
04900 ;Start of prom.
05000 LOC 174000
05100
05200 INITBL: STATUS ↔ 0
05300
05400 NINTER ↔ =32
05500 INTSCL ↔ 5
05600
05700 HSTLIM ↔ =48
05800
05900 377 ;End of INITBL flag.
00100 ;Power on reset.
00200 START: CLD
00300 LDXI STKSIZ ;Setup stack.
00400 TXS
00500
00600 LDAI 0
00700 LDXI ZAPEND
00800 RLOOP: STAZX 0 ;Reset ram.
00900 DEX
01000 BPL RLOOP
01100 STA DAC ;Clear DAC.
01200
01300 LDXI 370 ;-8.
01400 ZSR: STAZX FRICTN+10 ;Clear shared ram.
01500 INX
01600 BMI ZSR
01700
01800 TAY
01900 BEQ RSTDEF ;Jump
02000
02100
02200 DLOOP: INY
02300 LDAY INITBL ;Init ram.
02400 STAZX 0
02500 INY
02600
02700 RSTDEF: LDXY INITBL
02800 CPXI 377
02900 BNE DLOOP
03000
03100 STOP: SEI ;Go into stop mode.
03200 LDAI 0
03300 STAZ MODE ;Clear position servo enable, etc..
03400 JSR GETPOS ;Read encoder and convert to binary.
03500
03600 ;Sets the current position to the converted encoder value, the
03700 ;setpoint the same, clears the setpoint interpolating increment,
03800 ;and goes into stop mode.
03900 STAZ CURPOS ;Set the current position.
04000 STXZ CURPOS+1;Unlock.
04100
04200 STAZ SETPT ;Set the setpoint.
04300 STXZ SETPT+1
04400 STAZ OLDSP ;For CMDVEL.
04500 STXZ OLDSP+1
04600
04700 LDAI 75 ;I/O control bits for servo enable on,
04800 STAZ IOCTRL ;all others off.
04900 STA JCR
05000
05100 LDAI 0
05200 STAZ SETPT-1 ;Clear the setpoint extension,
05300 STAZ SETINC-1;the interpolator,
05400 STAZ SETINC
05500 STAZ SETINC+1
05600 STAZ CMDVEL ;and the commanded velocity.
05700 STAZ CMDVEL+1
05800
05900 STAZ DEFCMD ;Clear the deferred command flag.
06000
06100 CLI ;End of reset.
00100 RSTCKW: LDAI 377 ;Reset check word.
00200 LDXI 0
00300 SEI
00400 STAZ CKWORD ;Lock.
00500 STXZ CKWORD+1;Unlock.
00600 CLI
00700 ;Idle loop. Wait for command.
00800 IDLE: LDAZ CKWORD+1;Check for new check word.
00900 BEQ IDLE ;Not equal if bit 7 is complement of low byte.
01000
01100 SEC
01200 SEI
01300 ADCZ CKWORD ;Lock.
01400 LDXZ CKWORD+1;Unlock.
01500 CLI
01600 TAY
01700 BNE CKWDER ;Check word error.
01800 ;Check here for immediate or deferred.
01900 TXA ;Check for valid command.
02000 ORAI 3 ;3 for two commands and bit 0 = 0.
02100 ADCI 0 ;Carry = 1.
02200 BNE CKWDER ;Not a valid command.
02300
02400 LDAZ DEFCMD ;Check if no TICK?
02500 BNE NOTICK ;No response since last deferred command.
02600
02700 SEI
02800 LDYZ CMDPOS ;Read position for servo command.
02900 LDAZ CMDPOS+1;Unlock.
03000 CLI
03100
03200 STYZ SAVPOS ;Save it for later.
03300 STAZ SAVPOS+1
03400 ASLA ;Check for valid position.
03500 BCS CSET
03600 BMI CKWDER
03700 BPL GOODP
03800 CSET: BPL CKWDER
03900
04000 GOODP: STXZ DEFCMD ;Save deferred command pointer.
04100
04200 JMP RSTCKW ;Handshake with host via CKWORD.
04300
04400 CKWDER: LDAI 100 ;Set check word error flag.
04500 WSTAT: ORAZ STATUS
04600 STAZ STATUS
04700 JMP STOP
04800
04900 ;If status is in low byte. For host lockout.
05000 ; LDAI 100
05100 ; SEI
05200 ; ORAZ STATUS
05300 ; STAZ STATUS
05400 ; LDAZ STATUS+1
05500 ; CLI
05600 ; JMP STOP
05700
05800 NOTICK: LDAI 20 ;Set tick error flag.
05900 BNE WSTAT ;Jump.
00100 ;Clock tick interrupt.
00200 TICK: PHA ;Save state.
00300 TXA
00400 PHA
00500 TYA
00600 PHA
00700
00800 JSR GETPOS ;Read position and convert to binary.
00900
01000 SEC
01100 SBCZ CURPOS ;Subtract the old position
01200 STAZ CURVEL ;yielding the velocity.
01300 TXA ;High byte of binary position.
01400 SBCZ CURPOS+1;Unlock.
01500 STAZ CURVEL
01600
01700 STYZ CURPOS ;Update the current position.
01800 STXZ CURPOS+1;Unlock.
01900 DECZ HSTTMR ;Count the ticks since the last command
02000 BPL HOSTOK ;and check for timeout.
02100
02200 LDAI 0 ;Host dead. Stop.
02300 STAZ HSTTMR
02400 STAZ CMDVEL
02500 STAZ CMDVEL+1
02600 LDAI 40 ;Set host time out flag
02700 ORAZ STATUS
02800 STAZ STATUS
02900
03000 HOSTOK: LDAI 4
03100 BITZ IOCTRL ;If position mode is off,
03200 BNE INTVEL
03300 JMP CURSRV ;don't servo.
03400
03500 INTVEL: CLC ;Interpolate the velocity.
03600 LDAZ SETINC-1
03700 ADCZ OLDINC-1;IVEL ← OLDINC + SETINC.
03800 STAZ OLDINC-1
03900 LDAZ SETINC
04000 ADCZ OLDINC
04100 STAZ IVEL
04200 LDAZ SETINC+1
04300 ADCZ OLDINC+1
04400
04500 ASLZ OLDINC-1;IVEL ← IVEL * 4
04600 ROLZ IVEL
04700 ROLA
04800 ASLZ OLDINC-1
04900 ROLZ IVEL
05000 ROLA
05100 STAZ IVEL+1
05200
05300 LDAZ SETINC-1;OLDINC ← SETINC.
05400 STAZ OLDINC-1
05500 LDAZ SETINC
05600 STAZ OLDINC
05700 LDAZ SETINC+1
05800 STAZ OLDINC+1
00100 ;Interpolate the setpoints.
00200 INTRS: CLC
00300 LDAZ SETPT-1
00400 ADCZ SETINC-1;Add the increment to the setpoint.
00500 STAZ SETPT-1
00600 LDAZ SETPT
00700 ADCZ SETINC
00800 STAZ SETPT
00900 LDAZ SETPT+1
01000 ADCZ SETINC+1
01100 STAZ SETPT+1
01200
01300 DECZ INCTR ;Check if this is the last interpolation.
01400 BNE GPOSER
01500
01600 LDAI 0 ;Clear SETINC if done interpolating.
01700 STAZ SETINC-1
01800 STAZ SETINC
01900 STAZ SETINC+1
02000
02100 ;Calculate the position error.
02200 GPOSER: SEC
02300 LDAZ CURPOS ;POSERR ← CURPOS - SETPT.
02400 SBCZ SETPT
02500 STAZ POSERR
02600 LDAZ CURPOS+1
02700 SBCZ SETPT+1
02800 STAZ POSERR+1
00100 BITZ MODE ;If servo is disabled, we're
00200 BPL OOTOL ;automatically out of tolerance
00300
00400 LDAZ POSERR+1;Test the sign of pos error.
00500 BMI NEGPER
00600
00700 LDAZ POSTOL ;Positive. Compare with tol.
00800 CMPZ POSERR
00900 LDAZ POSTOL+1;Unlock.
01000 SBCZ POSERR+1
01100 BCS TOLOK ;In tolerance.
01200 BCC OOTOL ;Jump.
01300
01400 NEGPER: CLC ;Negative. Add the tolerance.
01500 LDAZ POSTOL ;Lock.
01600 ADCZ POSERR
01700 LDAZ POSTOL+1;Unlock.
01800 ADCZ POSERR+1
01900 BCS TOLOK ;In tolerance.
02000
02100 OOTOL: LDAZ IOCTRL ;Out of tolerance.
02200 ANDI 177 ;Turn off the in tolerance
02300 BNE WCNTRL ;indicator. Jump.
02400
02500 TOLOK: LDAZ IOCTRL ;In tolerance. Turn it on.
02600 ORAI 200
02700 WCNTRL: STAZ IOCTRL
02800 STA JCR ;Copy it to output.
02900
03000 BITZ MODE ;If intergration is disabled,
03100 BVC OOBAND ;turn it off.
03200 LDAZ POSERR+1;Test sign of position error.
03300 BMI ADTOL
03400
03500 LDAZ INTTOL ;Positive. Compare with tol.
03600 CMPZ POSERR
03700 LDAZ INTTOL+1;Unlock.
03800 SBCZ POSERR+1
03900 BCS INBAND ;In band. Turn on integrator.
04000 BCC OOBAND ;Jump.
04100
04200 ADTOL: CLC ;Negative. Add the tolerance.
04300 LDAZ INTTOL ;Lock.
04400 ADCZ POSERR
04500 LDAZ INTTOL+1;Unlock.
04600 ADCZ POSERR+1
04700 BCS INBAND ;Check if in band.
04800
04900 OOBAND: LDAZ IOCTRL ;Out of band. Turn off
05000 ORAI 10 ;integration by setting the
05100 ANDI 357 ;control bit. LSB servo off.
05200 BNE WCTRL2 ;Jump.
00100 INBAND: LDAI LSBENB ;In band. Is LSB servo enabled?
00200 BITZ MODE
00300 BEQ RCNTRL
00400
00500 LDAZ POSERR ;Yes. Is the error exactly 0?
00600 ORAZ POSERR+1
00700 BNE RCNTRL
00800
00900 LDAZ IOCTRL ;It is. Integration off, LSB
01000 ORAI 30 ;servo on.
01100 BNE WCTRL2 ;Jump.
01200
01300 RCNTRL: LDAZ IOCTRL ;LSB disabled or error
01400 ANDI 347 ;not zero. LSB servo off,
01500 ;integration on.
01600
01700 WCTRL2: STAZ IOCTRL
01800 STA JCR ;Output it.
01900
02000 ;Get the velocity error.
02100 CLC
02200 LDAZ VSUM
02300 ADCZ CURVEL ;VSUM ← VSUM + CURVEL.
02400 TAX
02500 LDAZ VSUM+1
02600 ADCZ CURVEL+1
02700 TAY
02800 TXA
02900 LDXZ VPTR ;Get velocity averaging index.
03000 SEC
03100 SBCZX VELTBL ;VSUM ← VSUM - VELTBL[VPTR].
03200 STAZ VSUM
03300 TYA
03400 SBCZX VELTBL+10
03500 STAZ VSUM+1
03600 TAY
03700
03800 LDAZ CURVEL ;VELTBL[VPTR] ← CURVEL.
03900 STAZX VELTBL
04000 LDAZ CURVEL+1
04100 STAZX VELTBL+10
04200 INX ;VPTR ← (VPTR + 1) .AND. (VTLEN - 1).
04300 TXA
04400 ANDI 7
04500 STAZ VPTR
04600
04700 SEC
04800 LDAZ VSUM
04900 SBCZ IVEL ;VELERR ← VSUM - IVEL.
05000 STAZ VELERR
05100 TYA ;A ← VSUM+1.
05200 SBCZ IVEL+1
05300
05400 LDXZ 3 ;Number of right shifts for divide by 8.
05500 VRSCL: CMPI 200 ;Extend sign.
05600 RORA ;A = VELERR+1.
05700 RORZ VELERR ;VELERR ← VELERR / 8.
05800 DEX
05900 BNE VRSCL
00100 LDYZ VELERR ;Get the velocity error,
00200 ;A = VELERR+1
00300 JSR LOG
00400 LDXZ FRICTN ;(Copy friction for multiply.)
00500 STXZ FTMP
00600 LDXZ FRICTN+1;Unlock.
00700 STXZ FTMP+1
00800 LDXI FTMP ;multiply by the friction
00900 JSR MULTIP ;coefficient,
01000 JSR EXP
01100
01200 TAX ;Save high byte.
01300 TYA ;Get low byte.
01400 CLC ;add the position error...
01500 ADCZ POSERR
01600 STAZ DACSIG
01700 TXA
01800 ADCZ POSERR+1
01900 STAZ DACSIG+1
02000
02100 CLC ;...and the gravity offset.
02200 LDAZ DACSIG
02300 ADCZ GRAVTY ;Lock.
02400 TAY ;Save low byte.
02500 LDAZ GRAVTY+1;Unlock.
02600 ADCZ DACSIG+1
02700
02800 JSR PUTDAC ;Put result out to the DAC.
02900
03000 CMDSP: LDAZ DEFCMD ;Check for a command.
03100 BEQ INTXIT
03200 ANDI 2 ;Low nibble command bit.
03300 TAX
03400 LDAX CMDTBL ;Get command address.
03500 STAZ DSPAT
03600 LDAX CMDTBL+1
03700 STAZ DSPAT+1
03800 JMPIN DSPAT ;Execute command.
03900
04000 CMDEND: LDAI 0 ;Done with deferred command.
04100 STAZ DEFCMD ;Reset command word.
04200 INTXIT: PLA ;Restore state and dismiss interrupt.
04300 TAY
04400 PLA
04500 TAX
04600 PLA
04700 RTI
04800
04900 CURSRV: LDAI 0 ;Not servoing ("Current mode")...
05000 STAZ SETPT-1 ;Make the setpoint track
05100 LDAZ CURPOS ;the current position in order to
05200 STAZ SETPT ;keep the arm from twitching when
05300 LDAZ CURPOS+1;the host enables the servo. Unlock.
05400 STAZ SETPT+1
05500 JMP CMDSP ;Go check on commands.
05600
05700 CMDTBL: ;DEFERRED COMMAND TABLE.
05800 CMDEND∧377 ;Nop.
05900 (CMDEND⊗-10)∧377
06000 CMDSRV∧377 ;Servo command.
06100 (CMDSRV⊗-10)∧377
00100 ;Deferred commands.
00200 CMDSRV: LDAZ MODE ;Servo command.
00300 ANDI 202 ;Test for servo enabled.
00400 CMPI 200
00500 BEQ ENBLD
00600 JMP CMDEND ;No. End this command.
00700
00800 ENBLD: LDAZ SAVPOS ;Enabled.
00900 LDXZ SAVPOS+1;Get commanded position.
01000 SEC
01100 SBCZ SETPT ;Get differance between next position
01200 STAZ SETINC ;and the last setpoint.
01300 TXA
01400 SBCZ SETPT+1
01500 LDXI 0
01600 STXZ SETPT-1 ;Clear setpoint and increment extentions.
01700 STXZ SETINC-1
01800 LDXZ INTSCL
01900
02000 SCAL: CMPI 200 ;Extend sign.
02100 RORA ;Divide the differance by the number of interpolations.
02200 RORZ SETINC
02300 RORZ SETINC-1
02400 DEX
02500 BNE SCAL
02600
02700 STAZ SETINC+1;Which yields the interpolating increment.
02800 LDAZ NINTER
02900 STAZ INCTR ;Setup the interpolator count.
03000 SEC
03100 LDAZ SAVPOS
03200 SBCZ OLDSP
03300 STAZ CMDVEL ;CMDVEL ← CMDPOS - OLDSP.
03400 LDAZ SAVPOS+1
03500 SBCZ OLDSP+1
03600 STAZ CMDVEL+1
03700 LDAZ SAVPOS
03800 STAZ OLDSP ;OLDSP ← CMDPOS.
03900 LDAZ SAVPOS+1
04000 STAZ OLDSP+1
04100
04200 LDAZ IOCTRL
04300 ORAI 44 ;Turn on servo and current mode enable bits.
04400 STAZ IOCTRL
04500 STA JCR ;Output it.
04600 LDAZ HSTLIM ;Reset host timer.
04700 STAZ HSTTMR
04800 JMP CMDEND
00100 ;Position conversion routine.
00200 GETPOS: LDY ENCL ;Read encoder.
00300 LDA ENCH
00400 EORI 377 ;Complement it.
00500 ;Convert from gray to binary.
00600 STAZ TH
00700 LSRA ;Shift by 1.
00800 EORZ TH
00900 STAZ TH
01000 TAX ;X ← high byte.
01100
01200 TYA
01300 EORI 377 ;Complement low byte.
01400 STAZ TL
01500 RORA
01600 EORZ TL
01700 STAZ TL
01800
01900 LSRZ TH ;Shift by 2.
02000 RORA
02100 LSRZ TH
02200 RORA
02300 EORZ TL
02400 STAZ TL
02500 TAY ;Y ← low byte.
02600
02700 TXA ;Get high byte.
02800 EORZ TH
02900 STAZ TH
03000
03100 LSRA ;Shift by 4.
03200 RORZ TL
03300 LSRA
03400 RORZ TL
03500 LSRA
03600 RORZ TL
03700 LSRA
03800 RORZ TL
03900
04000 EORZ TH
04100 STAZ TH
04200 TYA
04300 EORZ TL
04400 EORZ TH ;Shift by 8.
04500 TAY ;Save low byte.
04600
04700 LDXZ TH ;Get high byte.
04800 BITZ TH
04900 BVC POS ;Check if negative.
05000 TXA
05100 ORAI 200 ;Extend sign.
05200 TAX
05300
05400 POS: TYA ;Returns with position in A, Y (low) and X (high).
05500 RTS
00100 ;DAC output subroutine.
00200 ;Enter with 2 byte value in Y (low), A (high).
00300 ;Clobbers all registers, but the 8 bits the DAC got are returned in A.
00400 PUTDAC: BMI NEGDAC ;Assuming the last inst. loaded A.
00500 CPYI 200 ;Positive. Compare with 2↑7.
00600 SBCI 0
00700 BCC INRNGE
00800
00900 TOOHI: LDYI 177 ;Too high. Saturate positive.
01000 BNE INRNGE ;Jump.
01100
01200 NEGDAC: CPYI 200 ;Negative. Compare with -2↑7.
01300 SBCI 377
01400 BCS INRNGE
01500
01600 TOOLOW: LDYI 200 ;Too low. Saturate to -2↑7.
01700
01800 INRNGE: LDAY VETBL ;Straighting it.
01900 STA DAC ;Output 8 bits to the DAC.
02000 RTS
00100 ;Arithmetic routines.
00200 ;Enter with high byte in A, low in Y.
00300 ;Returns A = characteristic and sign, Y = mantissa.
00400 ;Clobbers X, LOGTMP, LOGTMP+1.
00500 LOG: STYZ LOGTMP ;Save the inputs.
00600 STAZ LOGTMP+1
00700
00800 LDXI 20+100 ;Init characteristic to 15.
00900 CMPI 0 ;Test sign of input.
01000 BPL POSIN
01100 SEC ;Negative. 2's complement it.
01200 LDAI 0
01300 SBCZ LOGTMP
01400 STAZ LOGTMP
01500 LDAI 0
01600 SBCZ LOGTMP+1
01700 POSIN: BNE NORML ;Is high byte zero?
01800 LDAZ LOGTMP ;Yes. Low byte?
01900 BEQ RTRN ;If so, return zero.
02000 LDYI 0 ;Low nonzero. Shift left one
02100 STYZ LOGTMP ;byte,
02200 LDXI 10+100 ;change characteristic to 7.
02300 NORML: DEX ;Normalize the number, counting the
02400 ASLZ LOGTMP ;characteristic down. When the
02500 ROLA ;first "1" shifts out, we've subtracted
02600 BCC NORML ;1 from the normalized number
02700 ASLZ LOGTMP ;(This rounds the result)
02800 ADCI =11 ;and are left with the fraction
02900 TAY ;Adding 11 to that is equivalent to
03000 TXA ;adding 0.043.
03100 ADCI 0 ;Propagate the carry into the
03200 ;characteristic.
03300 ASLA ;Insert the sign bit from the saved
03400 ASLZ LOGTMP+1;input.
03500 RORA
03600 RTRN: RTS ;Done.
03700
03800 ;Enter with sign and characteristic in A, mantissa in Y
03900 ;Returns 16-bit integer, low byte in Y, high in A.
04000 ;Clobbers X, LOGTMP, LOGTMP+1.
04100 EXP: STAZ LOGTMP+1;Save sign of input.
04200 ANDI 177 ;Mask it off.
04300 BEQ ZEROIN ;Zero characteristic returns
04400 TAX ;zero.
04500 TYA ;Get the mantissa...
04600 SEC
04700 SBCI =11 ;...subtract 0.043...
04800 STAZ LOGTMP ;(save this value)
04900 TXA ;...propagate the carry and get rid
05000 SBCI 100 ;of the XS-64 offset.
05100 BMI NEGIN ;If negative (value < 1.0)
05200 ;return zero.
05300 CMPI =15 ;Test for overflow (value>=2↑15
05400 BCS SATUR
05500 TAX ;...no. Number is in range.
05600 ADCI 370 ;Is characteristic below 8?
05700 BMI BLOATE
05800 TAX ;No. Reduce if by 8,
05900 JSR UNNORM ;unnormalize.
06000 BMI GETTMP ;Jump.
00100 BLOATE: JSR UNNORM ;Yes. Unnormalize, then
00200 ASLZ LOGTMP ;(round result)
00300 ADCI 0
00400 STAZ LOGTMP ;use result as low byte and
00500 LDAI 0 ;set high byte to zero.
00600
00700 GETTMP: LDYZ LOGTMP
00800 GTMP1: LDXZ LOGTMP+1;Test sign of input...
00900 BPL POSIGN
01000 STAZ LOGTMP+1;...negative. 2's complement
01100 LDAI 0 ;the result.
01200 SEC
01300 SBCZ LOGTMP
01400 TAY
01500 LDAI 0
01600 SBCZ LOGTMP+1
01700 POSIGN: RTS
01800
01900 NEGIN: LDAI 0 ;Set the result to zero if the
02000 ZEROIN: TAY ;input is negative.
02100 RTS
02200
02300 SATUR: LDYI 377 ;Saturate result to 2↑15 - 1 if
02400 STYZ LOGTMP ;input was 15 or more.
02500 LDAI 177
02600 BNE GTMP1 ;Jump.
02700
02800 UNNORM: LDAI 1 ;Unnormalize subroutine. Add 1
02900 BNE DECRX ;to the fraction. Jump.
03000
03100 SCALE: ASLZ LOGTMP ;Scale the fraction left by the
03200 ROLA ;amount of the characteristic.
03300 DECRX: DEX
03400 BPL SCALE
03500 RTS
00100 ;Enter with characteristic of multiplier in A,
00200 ;mantissa in Y, X pointing to a pair of base page
00300 ;locations containing the multiplicand (mantissa in the
00400 ;low byte).
00500 ;Returns the product in A and Y, same form as the
00600 ;multiplier. Leaves X unchanged. Clobbers LOGTMP and
00700 ;LOGTMP+1.
00800 MULTIP: PHA
00900 EORZX 1 ;Compute sign of result,
01000 STAZ LOGTMP+1 ;save it away.
01100 PLA
01200 ANDI 177 ;Mask off multiplier sign.
01300 BEQ ZEROIN ;If zero, return zero.
01400 STAZ LOGTMP
01500 TYA ;Add the two logarithms.
01600 CLC
01700 ADCZX 0
01800 TAY
01900 LDAZX 1
02000 ANDI 177 ;If multiplicand is zero,
02100 BEQ ZEROIN ;return a zero.
02200 ADCZ LOGTMP
02300 SEC
02400 SBCI 100 ;Correct the XS-64 offset.
02500 BPL INSIGN ;Result in range?
02600 ANDI 100 ;No. If underflow,
02700 BNE NEGIN ;return zero.
02800 LDAI 177 ;Overflow. Saturate to
02900 LDYI 377 ;highest magnitude.
03000
03100 INSIGN: ASLA ;Insert the sign of the result.
03200 ASLZ LOGTMP+1
03300 RORA
03400 RTS
03500
03600 ;Inverse function: 2's complement the magnitude part
03700 ;of a 15-bit logarithm.
03800 ;Enter with characteristic in A, mantissa in Y.
03900 ;Returns inverse in the same form. X unchanged.
04000 ;Clobbers LOGTMP and LOGTMP+1.
04100 INV: STYZ LOGTMP ;Pretty straightforward...
04200 STAZ LOGTMP+1
04300 SEC
04400 LDAI 0 ;Complement the number by
04500 SBCZ LOGTMP ;subtracting it from zero.
04600 TAY
04700 LDAI 0
04800 SBCZ LOGTMP+1
04900 JMP INSIGN ;Insert the original sign.
00100 ;DAC output table.
00200 LOC (.∨377)+1 ;For start of next page.
00300 VETBL: ;DAC output table.
00400 0 ↔ 20 ↔ 26 ↔ 32 ↔ 37 ↔ 43 ↔ 46 ↔ 50
00500 52 ↔ 54 ↔ 56 ↔ 57 ↔ 60 ↔ 62 ↔ 63 ↔ 64
00600 65 ↔ 66 ↔ 67 ↔ 70 ↔ 71 ↔ 72 ↔ 73 ↔ 74
00700 75 ↔ 76 ↔ 76 ↔ 77 ↔ 100 ↔ 101 ↔ 102 ↔ 103
00800 104 ↔ 104 ↔ 105 ↔ 106 ↔ 107 ↔ 107 ↔ 110 ↔ 111
00900 112 ↔ 112 ↔ 113 ↔ 114 ↔ 115 ↔ 115 ↔ 116 ↔ 117
01000 117 ↔ 120 ↔ 121 ↔ 121 ↔ 122 ↔ 123 ↔ 124 ↔ 124
01100 125 ↔ 126 ↔ 127 ↔ 127 ↔ 130 ↔ 131 ↔ 131 ↔ 132
01200 133 ↔ 133 ↔ 134 ↔ 135 ↔ 135 ↔ 136 ↔ 136 ↔ 137
01300 140 ↔ 140 ↔ 141 ↔ 142 ↔ 142 ↔ 143 ↔ 143 ↔ 144
01400 145 ↔ 145 ↔ 146 ↔ 146 ↔ 147 ↔ 150 ↔ 150 ↔ 151
01500 151 ↔ 152 ↔ 153 ↔ 153 ↔ 154 ↔ 154 ↔ 155 ↔ 156
01600 156 ↔ 157 ↔ 160 ↔ 160 ↔ 161 ↔ 161 ↔ 162 ↔ 162
01700 163 ↔ 164 ↔ 164 ↔ 165 ↔ 165 ↔ 166 ↔ 166 ↔ 167
01800 167 ↔ 170 ↔ 170 ↔ 171 ↔ 171 ↔ 172 ↔ 172 ↔ 173
01900 173 ↔ 174 ↔ 174 ↔ 175 ↔ 176 ↔ 176 ↔ 177 ↔ 177
02000
02100 200 ↔ 200 ↔ 200 ↔ 201 ↔ 201 ↔ 202 ↔ 203 ↔ 203
02200 204 ↔ 204 ↔ 205 ↔ 205 ↔ 206 ↔ 206 ↔ 207 ↔ 207
02300 210 ↔ 210 ↔ 211 ↔ 211 ↔ 212 ↔ 212 ↔ 213 ↔ 213
02400 214 ↔ 215 ↔ 215 ↔ 216 ↔ 216 ↔ 217 ↔ 217 ↔ 220
02500 221 ↔ 221 ↔ 222 ↔ 223 ↔ 223 ↔ 224 ↔ 224 ↔ 225
02600 226 ↔ 226 ↔ 227 ↔ 227 ↔ 230 ↔ 231 ↔ 231 ↔ 232
02700 232 ↔ 233 ↔ 234 ↔ 234 ↔ 235 ↔ 235 ↔ 236 ↔ 237
02800 237 ↔ 240 ↔ 241 ↔ 241 ↔ 242 ↔ 242 ↔ 243 ↔ 244
02900 244 ↔ 245 ↔ 246 ↔ 246 ↔ 247 ↔ 250 ↔ 250 ↔ 251
03000 252 ↔ 253 ↔ 253 ↔ 254 ↔ 255 ↔ 256 ↔ 256 ↔ 257
03100 260 ↔ 260 ↔ 261 ↔ 262 ↔ 262 ↔ 263 ↔ 264 ↔ 265
03200 265 ↔ 266 ↔ 267 ↔ 270 ↔ 270 ↔ 271 ↔ 272 ↔ 273
03300 273 ↔ 274 ↔ 275 ↔ 276 ↔ 277 ↔ 300 ↔ 301 ↔ 301
03400 302 ↔ 303 ↔ 304 ↔ 305 ↔ 306 ↔ 307 ↔ 310 ↔ 311
03500 312 ↔ 313 ↔ 314 ↔ 315 ↔ 317 ↔ 320 ↔ 322 ↔ 323
03600 325 ↔ 327 ↔ 331 ↔ 334 ↔ 340 ↔ 345 ↔ 351 ↔ 357
03700
03800 NMI ← START ;Reset??
03900 ;Interrupt vectors.
04000 LOC 177772
04100 NMI∧377
04200 (NMI⊗-10)∧377
04300 START∧377
04400 (START⊗-10)∧377
04500 TICK∧377
04600 (TICK⊗-10)∧377
04700 END